/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-11 13:52:35
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-01 02:20:52
 * @Description: ...
 * @Reference: ...
 */

#ifndef SRC_STEREO_CONVERTER_H
#define SRC_STEREO_CONVERTER_H
#include <string>
#include "converter.h"

namespace kaist_publisher {

class StereoConverter : public Converter {
public:
    typedef std::shared_ptr<StereoConverter> Ptr;
    StereoConverter(ros::NodeHandle& nh, const std::string& dataset_dir,
                    const std::string& save_dir, const std::string& left_topic,
                    const std::string& right_topic);
    virtual ~StereoConverter() = default;

    int Convert() override;

    std::string default_stamp_file = "sensor_data/stereo_stamp.csv";
    std::string default_left_data_dir = "image/stereo_left";
    std::string default_right_data_dir = "image/stereo_right";

    int64_t Init_Publish();
    int64_t Publish();
    void Publish(int64_t& timestamp, sensor_msgs::Image::Ptr& left_image,
                 sensor_msgs::Image::Ptr& right_image);

    ros::Publisher pub_stereo_left_;
    ros::Publisher pub_stereo_right_;
    std::vector<int64_t> all_stamps_;
    int idx_ = 0;

private:
    std::string left_topic_;
    std::string left_bag_name_;
    std::string right_topic_;
    std::string right_bag_name_;

    void Convert(const std::string& stamp_file, const std::string& data_dir,
                 const std::string& bag_file, const std::string& topic,
                 const std::string& frame_id);
};

}  // namespace kaist_publisher
#endif  // SRC_STEREO_CONVERTER_H
